use std::collections::BTreeMap;

use serde::{Deserialize, Serialize};

use crate::{Error, Result};

pub trait FromBytes<'a>: Deserialize<'a> {
    fn from_bytes(buf: &'a [u8]) -> Result<Self> {
        rmp_serde::from_read_ref(buf).map_err(|e| Error::from(e))
    }
}

pub trait ToBytes: Serialize {
    fn to_bytes(&self) -> Result<Vec<u8>> {
        rmp_serde::to_vec(&self).map_err(|e| Error::from(e))
    }
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub enum UpMessage {
    CreateVehicle(CreateVehicle),
    GetVehicle(GetVehicle),
    SetVehicleTransform(VehicleTransform),
    DetachVehicle,
    VehicleControl(VehicleControl),
    Empty,
}

impl<'a> FromBytes<'a> for UpMessage {}

impl ToBytes for UpMessage {}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct CreateVehicle {
    pub client_id: String,
    pub vehicle_type: String,
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct GetVehicle {
    pub client_id: String,
    pub token: String,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct VehicleTransform {
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub enum DownMessage {
    Response(Response),
    Sensors(Sensors),
}

impl DownMessage {
    pub fn response(ret: bool) -> Self {
        Self::Response(Response { ret })
    }
}

impl<'a> FromBytes<'a> for DownMessage {}

impl ToBytes for DownMessage {}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct Response {
    pub ret: bool,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct Sensors {
    pub test_result: Option<TestResult>,
    pub localization: Option<LocalizationInfo>,
    pub perception: Option<PerceptionInfo>,
    pub chassis: Option<ChassisInfo>,
    pub collision_event: Option<CollisionEvent>,
    pub lane_invasion_event: Option<LaneInvasionEvent>,
    pub camera_images: Vec<Image>,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct TestResult {
    pub success: bool,
    pub criteria: BTreeMap<String, bool>,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct LocalizationInfo {
    pub timestamp: u64,
    pub cartesian_lla: (f64, f64, f64),
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
    pub linear_vel_xyz: (f64, f64, f64),
    pub linear_accel_xyz: (f64, f64, f64),
    pub angular_vel_xyz: (f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct PerceptionInfo {
    pub timestamp: u64,
    pub obstacles: Vec<ObstacleInfo>,
    pub traffic_light: Option<TrafficLightInfo>,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct ObstacleInfo {
    pub id: String,
    pub o_type: ObstacleType,
    pub is_static: bool,
    pub bounding_box: BoundingBox,
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
    pub linear_vel_xyz: (f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub enum ObstacleType {
    Unknow,
    Vehicle,
    Pedestrian,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct BoundingBox {
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
    pub extent_xyz: (f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct TrafficLightInfo {
    pub timestamp: u64,
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
    pub state: TrafficLightState,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub enum TrafficLightState {
    Red,
    Yellow,
    Green,
    Off,
    Unknown,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct ChassisInfo {
    pub timestamp: u64,
    pub control: VehicleControl,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct VehicleControl {
    pub throttle: f64,
    pub steer: f64,
    pub brake: f64,
    pub hand_brake: bool,
    pub reverse: bool,
    pub manual_gear_shift: bool,
    pub gear: i32,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct CollisionEvent {
    pub timestamp: u64,
    pub normal_impulse: (f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct LaneInvasionEvent {
    pub timestamp: u64,
    pub lane_change: Vec<LaneChangeType>,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub enum LaneChangeType {
    None,
    Right,
    Left,
    Both,
}

#[derive(Clone, Debug, Deserialize, Serialize)]
pub struct Image {
    pub timestamp: u64,
    pub camera_id: String,
    pub width: u32,
    pub height: u32,
    pub is_gray: bool,
    pub data: Vec<u8>,
}

impl Image {
    pub fn compress_data(&mut self) {
        self.data = zstd::bulk::compress(&self.data[..], 1).unwrap();
    }

    pub fn decompress_data(&mut self) -> Result<()> {
        let cap = if self.is_gray {
            self.width * self.height
        } else {
            self.width * self.height * 3
        } as usize;

        self.data = zstd::bulk::decompress(&self.data[..], cap)
            .map_err(|_| Error::new("Decompress error".to_string()))?;

        Ok(())
    }
}
